// --------------------------------------------------------------------------------------------------------------------
// <copyright file="KukaLBR4KinematicConfigurations.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------

namespace Microsoft.Robotics.Hardware.KukaLBR
{
    using System;
    using System.Collections.Generic;
    using Microsoft.Robotics.Geometry;
    using Microsoft.Robotics.Kinematics;
    using Microsoft.Robotics.Numerics;
    using ShoNS.Array;

    /// <summary>
    /// A class that includes functions to create the kinematic chain for a KUKA arm.
    /// Both 6-dof and 7-dof versions are available.
    /// The physical structure of the 6-dof version is the same as the 7-dof arm except
    /// that the third joint in the 7-dof is fixed in the 6-dof version.
    /// </summary>
    public class KukaLBR4KinematicConfigurations
    {
        /// <summary>
        /// Create a kinematic chain for 7DOF Kuka arm
        /// The arm is straight up at its zero pose
        /// </summary>
        /// <param name="endEffectorOffsetInLastJoint">Offset of the end effector in last joint, default is for Robotiq hand</param>
        /// <returns>A kinematic chain</returns>
        public static KinematicChain CreateKuka7DOFKinematicChain(Matrix4 endEffectorOffsetInLastJoint = null)
        {
            uint numJoints = 7;

            // create the joint list
            Joint[] jointList = new Joint[numJoints];
            jointList[0] = new Joint(DHParameter.CreateDHParameterForRevoluteJoint(0, 0, KukaLBR4IK6DOFClosedForm.L01), 0, -170.0 * MathConstants.Degrees2Radians, 170.0 * MathConstants.Degrees2Radians);
            jointList[1] = new Joint(DHParameter.CreateDHParameterForRevoluteJoint(MathConstants.PIOverTwo, 0, 0), Math.PI, -120.0 * MathConstants.Degrees2Radians, 120.0 * MathConstants.Degrees2Radians);
            jointList[2] = new Joint(DHParameter.CreateDHParameterForRevoluteJoint(MathConstants.PIOverTwo, 0, KukaLBR4IK6DOFClosedForm.L23), 0, -170.0 * MathConstants.Degrees2Radians, 170.0 * MathConstants.Degrees2Radians);
            jointList[3] = new Joint(DHParameter.CreateDHParameterForRevoluteJoint(MathConstants.PIOverTwo, 0, 0), 0, -120.0 * MathConstants.Degrees2Radians, 120.0 * MathConstants.Degrees2Radians);
            jointList[4] = new Joint(DHParameter.CreateDHParameterForRevoluteJoint(-MathConstants.PIOverTwo, 0, KukaLBR4IK6DOFClosedForm.L34), 0, -170.0 * MathConstants.Degrees2Radians, 170.0 * MathConstants.Degrees2Radians);
            jointList[5] = new Joint(DHParameter.CreateDHParameterForRevoluteJoint(-MathConstants.PIOverTwo, 0, 0), 0, -120.0 * MathConstants.Degrees2Radians, 120.0 * MathConstants.Degrees2Radians);
            jointList[6] = new Joint(DHParameter.CreateDHParameterForRevoluteJoint(MathConstants.PIOverTwo, 0, 0), 0, -170.0 * MathConstants.Degrees2Radians, 170.0 * MathConstants.Degrees2Radians);

            // create the collision model for each link as well as the end effector
            double sphereRadius = 0.08;

            // link0 which is controlled by joint0
            List<GeometryElement3D> link0Elements = new List<GeometryElement3D>();
            SphereElement l0s0 = new SphereElement(new Vector3(0, 0, -200.5 / 1000.0), 0.1);
            SphereElement l0s1 = new SphereElement(new Vector3(0, 0.03, -110.5 / 1000.0), sphereRadius);
            SphereElement l0s2 = new SphereElement(new Vector3(0, 0.02, -10.5 / 1000.0), sphereRadius);
            link0Elements.Add(l0s0);
            link0Elements.Add(l0s1);
            link0Elements.Add(l0s2);
            GeometryElementGroup3D link0Group = new GeometryElementGroup3D(link0Elements);

            // link1 which is controlled by jont1
            List<GeometryElement3D> link1Elements = new List<GeometryElement3D>();
            SphereElement l1s0 = new SphereElement(new Vector3(0, 0, 0.04), sphereRadius);
            SphereElement l1s1 = new SphereElement(new Vector3(0, -0.1, 0.02), sphereRadius);
            SphereElement l1s2 = new SphereElement(new Vector3(0, -0.2, 0), sphereRadius);
            link1Elements.Add(l1s0);
            link1Elements.Add(l1s1);
            link1Elements.Add(l1s2);
            GeometryElementGroup3D link1Group = new GeometryElementGroup3D(link1Elements);

            // link2 which is controlled by joint2
            List<GeometryElement3D> link2Elements = new List<GeometryElement3D>();
            SphereElement l2s0 = new SphereElement(new Vector3(0, 0.025, -0.11), sphereRadius);
            SphereElement l2s1 = new SphereElement(new Vector3(0, 0.03, 0), sphereRadius);
            link2Elements.Add(l2s0);
            link2Elements.Add(l2s1);
            GeometryElementGroup3D link2Group = new GeometryElementGroup3D(link2Elements);

            // link3 which is controlled by joint3
            List<GeometryElement3D> link3Elements = new List<GeometryElement3D>();
            SphereElement l3s0 = new SphereElement(new Vector3(0, 0, 0.05), sphereRadius);
            SphereElement l3s1 = new SphereElement(new Vector3(0, 0.11, 0.02), sphereRadius);
            SphereElement l3s2 = new SphereElement(new Vector3(0, 0.2, 0), sphereRadius);
            link3Elements.Add(l3s0);
            link3Elements.Add(l3s1);
            link3Elements.Add(l3s2);
            GeometryElementGroup3D link3Group = new GeometryElementGroup3D(link3Elements);

            // link4 which is controlled by joint4
            List<GeometryElement3D> link4Elements = new List<GeometryElement3D>();
            SphereElement l4s0 = new SphereElement(new Vector3(0, -0.02, -0.1), 0.07);
            link4Elements.Add(l4s0);
            GeometryElementGroup3D link4Group = new GeometryElementGroup3D(link4Elements);

            // link5 which is controlled by joint5
            List<GeometryElement3D> link5Elements = new List<GeometryElement3D>();
            SphereElement l5s0 = new SphereElement(new Vector3(0, 0, 0), sphereRadius);
            SphereElement l5s1 = new SphereElement(new Vector3(0, 0, -0.02), sphereRadius);
            link5Elements.Add(l5s0);
            link5Elements.Add(l5s1);
            GeometryElementGroup3D link5Group = new GeometryElementGroup3D(link5Elements);

            // putting all together
            // link6 does not have a collision model associated since it is a very small link
            // and is covered by the collision model of link5
            List<GeometryElementGroup3D> armCollisionModel = new List<GeometryElementGroup3D>();
            armCollisionModel.Add(link0Group);
            armCollisionModel.Add(link1Group);
            armCollisionModel.Add(link2Group);
            armCollisionModel.Add(link3Group);
            armCollisionModel.Add(link4Group);
            armCollisionModel.Add(link5Group);
            armCollisionModel.Add(null);

            // the collision model of the end effector
            List<GeometryElement3D> endEffectorCollisionModel = new List<GeometryElement3D>();

            // palm
            SphereElement palm0 = new SphereElement(new Vector3(-0.029, -0.029, -0.01), 0.045);
            SphereElement palm1 = new SphereElement(new Vector3(0.029, -0.029, -0.01), 0.045);
            SphereElement palm2 = new SphereElement(new Vector3(-0.025, 0.025, -0.01), 0.045);
            SphereElement palm3 = new SphereElement(new Vector3(0.025, 0.025, -0.01), 0.045);

            endEffectorCollisionModel.Add(palm0);
            endEffectorCollisionModel.Add(palm1);
            endEffectorCollisionModel.Add(palm2);
            endEffectorCollisionModel.Add(palm3);
            GeometryElementGroup3D robotiqHandCollisionModel = new GeometryElementGroup3D(endEffectorCollisionModel);

            string[] linkNameList = new string[8];
            linkNameList[0] = "base";
            linkNameList[1] = "link0";
            linkNameList[2] = "link1";
            linkNameList[3] = "link2";
            linkNameList[4] = "link3";
            linkNameList[5] = "link4";
            linkNameList[6] = "link5";
            linkNameList[7] = "link6";

            string[] linkGeometryPaths = new string[8];
            linkGeometryPaths[0] = @"Data\RobotModel\LBR\base.off";
            linkGeometryPaths[1] = @"Data\RobotModel\LBR\link0.off";
            linkGeometryPaths[2] = @"Data\RobotModel\LBR\link1.off";
            linkGeometryPaths[3] = @"Data\RobotModel\LBR\link2.off";
            linkGeometryPaths[4] = @"Data\RobotModel\LBR\link3.off";
            linkGeometryPaths[5] = @"Data\RobotModel\LBR\link4.off";
            linkGeometryPaths[6] = @"Data\RobotModel\LBR\link5.off";
            linkGeometryPaths[7] = @"Data\RobotModel\LBR\link6.off";

            string endEffectorName = "Robotiq3";

            string endEffectorGeometryPath = @"Data\RobotModel\LBR\robotiq3.off";

            // create a kinematic chain
            KinematicChain kc = new KinematicChain(jointList, linkNameList, linkGeometryPaths, endEffectorName, endEffectorGeometryPath, armCollisionModel, robotiqHandCollisionModel);

            // where the EE is w.r.t. the last joint
            Matrix4 endEffectorInLastJoint = Matrix4.Identity();
            if (null == endEffectorOffsetInLastJoint)
            {
                endEffectorInLastJoint = HomogenousTransform.CreateFromTranslation(new Vector3(5.657 / 1000.0, 5.657 / 1000.0, KukaLBR4IK6DOFClosedForm.L6E));
                endEffectorInLastJoint[0, 0] = -1;
                endEffectorInLastJoint[1, 1] = -1;
            }

            kc.SetEndEffectorOffsetInLastJoint(endEffectorInLastJoint);

            return kc;
        }

        /// <summary>
        /// Create a kinematic chain for 6DOF Kuka arm
        /// The arm is horizontal at its zero pose
        /// </summary>
        /// <param name="endEffectorOffsetInLastJoint">Offset of the end effector in last joint, default is for Robotiq hand</param>
        /// <returns>A kinematic chain</returns>
        public static KinematicChain CreateKuka6DOFKinematicChain(Matrix4 endEffectorOffsetInLastJoint = null)
        {
            uint numJoints = 6;

            // create the joint list
            Joint[] jointList = new Joint[numJoints];
            jointList[0] = new Joint(DHParameter.CreateDHParameterForRevoluteJoint(0, 0, KukaLBR4IK6DOFClosedForm.L01), 0, -170.0 * MathConstants.Degrees2Radians, 170.0 * MathConstants.Degrees2Radians);
            jointList[1] = new Joint(DHParameter.CreateDHParameterForRevoluteJoint(MathConstants.PIOverTwo, 0, 0), 0, -120.0 * MathConstants.Degrees2Radians, 120.0 * MathConstants.Degrees2Radians);
            jointList[2] = new Joint(DHParameter.CreateDHParameterForRevoluteJoint(Math.PI, KukaLBR4IK6DOFClosedForm.L23, 0), MathConstants.PIOverTwo, -120.0 * MathConstants.Degrees2Radians, 120.0 * MathConstants.Degrees2Radians);
            jointList[3] = new Joint(DHParameter.CreateDHParameterForRevoluteJoint(MathConstants.PIOverTwo, 0, KukaLBR4IK6DOFClosedForm.L34), 0, -170.0 * MathConstants.Degrees2Radians, 170.0 * MathConstants.Degrees2Radians);
            jointList[4] = new Joint(DHParameter.CreateDHParameterForRevoluteJoint(MathConstants.PIOverTwo, 0, 0), 0, -120.0 * MathConstants.Degrees2Radians, 120.0 * MathConstants.Degrees2Radians);
            jointList[5] = new Joint(DHParameter.CreateDHParameterForRevoluteJoint(-MathConstants.PIOverTwo, 0, 0), 0, -170.0 * MathConstants.Degrees2Radians, 170.0 * MathConstants.Degrees2Radians);

            // create a kinematic chain
            KinematicChain kc = new KinematicChain(jointList);

            // where the EE is w.r.t. the last joint
            Matrix4 endEffectorInLastJoint = (null == endEffectorOffsetInLastJoint) ? HomogenousTransform.CreateFromTranslation(new Vector3(0, 0, KukaLBR4IK6DOFClosedForm.L6E)) : endEffectorOffsetInLastJoint;
            kc.SetEndEffectorOffsetInLastJoint(endEffectorInLastJoint);

            return kc;
        }

        /// <summary>
        /// Some default joint configurations that can be used as good IK seeds.
        /// These joint configurations were collected during our pick-and-place tasks when
        /// the arm was at poses that look natural to the human data collector.
        /// These typical joint configurations do not contain ALL potentially good
        /// joint configurations and are not expected to work with ALL pick-and-place
        /// tasks.  We have them here only for our current pick-and-place tests that
        /// we run every day.
        /// </summary>
        /// <returns>A list of good seed joint configurations</returns>
        public static List<double[]> GetKuka7DOFDefaultIKSeeds()
        {
            List<double[]> configList = new List<double[]>();
            configList.Add(new double[] { 0.000167, 0.533, 0.00273, -1.95, 0.0648, -0.909, -0.815 });
            configList.Add(new double[] { 0.00885, 0.608, -0.0172, -0.913, 0.011, 1.62, -0.792 });
            configList.Add(new double[] { -0.588, -0.629, 2.18, -1.27, 0.503, 1.49, -1.12 });
            configList.Add(new double[] { -0.307, 0.674, 0.653, -0.914, -0.39, 1.66, -0.59 });
            configList.Add(new double[] { 1.05, 0.652, 0.63, -1.32, -0.379, 1.29, -0.54 });
            configList.Add(new double[] { 0.927, 0.731, 0.405, -0.962, -0.247, 1.51, -0.644 });
            configList.Add(new double[] { 0.616, 0.599, 0.851, -1.5, -0.459, 1.26, -0.451 });
            configList.Add(new double[] { 0.7, 0.817, 0.708, -1.63, -0.631, 0.935, -0.334 });
            configList.Add(new double[] { 0.604, 0.535, 0.894, -1.37, -0.413, 1.43, -0.49 });
            configList.Add(new double[] { -1.3, -0.57, 2.66, -1.4, 0.264, 1.24, -0.968 });

            configList.Add(new double[] { 1.5, 0.533, 0.00267, -1.95, 0.065, -0.909, -0.814 });
            configList.Add(new double[] { 2, 0.534, 0.00256, -1.95, 0.0649, -0.909, -0.815 });
            configList.Add(new double[] { 2, 0.7, 0.00256, -1.8, 0.0649, -0.909, -0.815 });
            configList.Add(new double[] { 1, 0.7, 0.00256, -1.8, 0.0649, -0.909, -0.815 });

            return configList;
        }

        /// <summary>
        /// A function that generates equivalent joint configurations based on the current joint configuration.
        /// End effector pose remains the same for all these equivalent joint configurations.
        /// This function is absolutely hardware kinematic specific.  Some arms may not have an
        /// equivalent generator like this.
        /// </summary>
        /// <param name="jointValues">Current joint configuration</param>
        /// <returns>Other joint values which are equivalent to the current one</returns>
        public static List<DoubleArray> KukaLBR4EquivalentJointConfigurationGenerator(DoubleArray jointValues)
        {
            List<DoubleArray> pool = new List<DoubleArray>();

            pool.Add(jointValues);

            DoubleArray eq1 = DoubleArray.From(jointValues);
            eq1[0] += Math.PI;
            eq1[1] = -eq1[1];
            eq1[2] += Math.PI;

            DoubleArray eq2 = DoubleArray.From(jointValues);
            eq2[4] += Math.PI;
            eq2[5] = -eq2[5];
            eq2[6] += Math.PI;

            DoubleArray eq3 = DoubleArray.From(jointValues);
            eq3[0] += Math.PI;
            eq3[1] = -eq3[1];
            eq3[2] += Math.PI;
            eq3[4] += Math.PI;
            eq3[5] = -eq3[5];
            eq3[6] += Math.PI;

            KinematicChain kc = CreateKuka7DOFKinematicChain();

            // Since each joint has its limit, it is necessary to check if
            // equivalent solutions satisfy joint limit requirements.
            // We check these requirements as follows based on the kinematic
            // chain of the Kuka arm which is built above.
            if (InverseKinematicsNumerical.NormalizeSolutionAndCheckWithJointLimits(eq1, kc))
            {
                pool.Add(eq1);
            }

            if (InverseKinematicsNumerical.NormalizeSolutionAndCheckWithJointLimits(eq2, kc))
            {
                pool.Add(eq2);
            }

            if (InverseKinematicsNumerical.NormalizeSolutionAndCheckWithJointLimits(eq3, kc))
            {
                pool.Add(eq3);
            }

            return pool;
        }
    }
}
